using System;
using System.Collections.Generic;
using System.Text;
using System.IO;
using G3.Controller;

namespace G3.State
{
    /*
    public struct Quad
    {
        public double x1;
        public double x2; // [ x1 x2 ]
        public double y1; // [ y1 y2 ]
        public double y2;

        public Quad(double x1, double x2, double y1, double y2)
        {
            this.x1 = x1; this.x2 = x2; this.y1 = y1; this.y2 = y2;
        }
    }

    public struct Pair
    {
        public double x; // [ x ]
        public double y; // [ y ]

        public Pair(double x, double y)
        {
            this.x = x;
            this.y = y;
        }

        static public Pair operator+(Pair p1, Pair p2)
        {
            return new Pair(p1.x + p2.x, p1.y + p2.y);
        }

        static public Pair operator-(Pair p1, Pair p2)
        {
            return new Pair(p1.x - p2.x, p1.y - p2.y);
        }

        static public Pair operator-(Pair p, double d)
        {
            return new Pair(p.x - d, p.y - d);
        }

        static public Pair operator*(double scalar, Pair p)
        {
            return new Pair(scalar*p.x,scalar*p.y);
        }

        static public Pair operator*(Quad matrix, Pair vector)
        {
            return new Pair(matrix.x1*vector.x+matrix.x2*vector.y,
                            matrix.y1*vector.x+matrix.y2*vector.y);
        }

        static public double operator*(Pair p1, Pair p2)
        {
            return p1.x * p2.x + p1.y * p2.y;
        }

        public double Abs()
        {
            return Math.Sqrt(x * x + y * y);
        }
    }
    */
    /*
    class Car
    {
        private double weight; //kg
        private double throttleDeadzone = 0.01;
        private double engineStrength = 1.0;
        private double brakeStrength = 1.0;
        private double steerStrength = 0.01;

        public double Weight
        {
            get
            {
                return weight;
            }
            set
            {
                weight = value;
            }
        }

        public Pair WheelPosition(double orientation, int wheelNumber)
        {
            return new Pair(0.0, 0.0);
        }

        public Pair Force(double throttle, double wheel, double orientation, double speed)
        {
            Pair p = new Pair(0.0, 0.0);
            p.x = speed * wheel * steerStrength;
            if (Math.Abs(throttle) < throttleDeadzone) p.y = 0.0;
            else if (throttle < 0.0) p.y = brakeStrength * throttle;
            else p.y = engineStrength * throttle;
            double c = Math.Cos(orientation);
            double s = Math.Sin(orientation);
            Quad rotation = new Quad(c,-s,s,c);
            Pair force = rotation * p;
            return force;
        }
    }
    */

    class State
    {
        public const double EPSILON = 0.000001;
        public const int STORING_DEPTH = 5;

        private List<StateVars> stateVars;

        private Pair carPosition; // meters
        private Pair carVelocity; // meters per sec
        private Pair carOrientation; //vector
        private double throttle; // -100.0 .. +100.0
        private double wheel; // -100.0 .. +100.0
        private Car car;
        private double time;
        private double distance;
        private double timePenalty;
        private double distancePenalty;

        public struct StateVars
        {
            public Pair carPosition;
            public Pair carVelocity;
            public double carOrientation;

            public StateVars(Pair pos, Pair vel, double ori)
            {
                carPosition = pos;
                carVelocity = vel;
                carOrientation = ori;
            }
        }

        //StreamWriter sw;

        public State()
        {
            stateVars = new List<StateVars>(STORING_DEPTH);
            carVelocity = new Pair(0.0, 0.0);
            carOrientation = new Pair(-1.0, 0.0);
            throttle = 0.0;
            wheel = 0.0;
            car = new Car();
            time = distance = timePenalty = distancePenalty = 0.0;
            stateVars.Add(CurrentStateVars);
            //sw = new StreamWriter("log.txt"); 
        }

        private void updateStateVars()
        {
            stateVars.Insert(0, CurrentStateVars);
            if (stateVars.Count > STORING_DEPTH)
            {
                stateVars.RemoveAt(STORING_DEPTH);
            }
        }

        public double Time
        {
            get
            {
                return time;
            }
        }

        public double Distance
        {
            get
            {
                return distance;
            }
        }

        public double TimePenalty
        {
            get
            {
                return timePenalty;
            }
        }

        public double DistancePenalty
        {
            get
            {
                return distancePenalty;
            }
        }

        public StateVars GetStateVars(int idx)
        {
            idx *= -1;
            if (idx < stateVars.Count)
            {
                return stateVars[idx];
            }
            else
            {
                return new StateVars(new Pair(0.0, 0.0), new Pair(0.0, 0.0), 0.0);
            }
        }

        public StateVars CurrentStateVars
        {
            get
            {
                return new StateVars(carPosition, carVelocity, CarOrientationDeg);
            }
        }

        public Pair CarPosition
        {
            get
            {
                return carPosition;
            }
        }

        public double CarOrientationRadian
        {
            get
            {
                return MyAtan(carOrientation.y, carOrientation.x);
            }
        }

        public double CarOrientationDeg
        {
            get
            {
                return (MyAtan(carOrientation.y, carOrientation.x) / Math.PI) * 180.0;
            }
        }

        public double Throttle
        {
            get
            {
                return throttle;
            }
            set
            {
                throttle = value;
            }
        }

        public double Wheel
        {
            get
            {
                return wheel;
            }
            set
            {
                wheel = value;
            }
        }

        public double CarSpeed
        {
            get
            {
                return carVelocity.Abs(); //TODO *3.6
            }
        }

        public Pair CarVelocity
        {
            get
            {
                return carVelocity;
            }
        }

        public void Init()
        {
            stateVars = new List<StateVars>(STORING_DEPTH);
            carPosition = new Pair(Map.StartPos.x, -Map.StartPos.y);
            carVelocity = new Pair(0.0, 0.0);
            carOrientation = new Pair(-1.0, 0.0);
            throttle = 0.0;
            wheel = 0.0;
            time = distance = timePenalty = distancePenalty = 0.0;
            car = new Car();
            stateVars.Add(CurrentStateVars);
        }

        /// <summary>
        /// Computes the required wheel and throttle values
        /// </summary>
        /// <param name="source"></param>
        /// <param name="destination"></param>
        /// <param name="millisec"></param>
        /// <returns></returns>
        public Pair ArcSim(StateVars source, StateVars destination, double millisec)
        {
            millisec /= 1000.0;
            Pair a = new Pair(0.0, 0.0);
            double r = 1.0 / millisec;
            Pair p1 = source.carPosition;
            Pair p2 = destination.carPosition;
            Pair v1 = source.carVelocity;
            a = 2 * r * (r * (p2 - p1) - v1); //required accel
            double speed = v1.Abs();
            Field f = Map.GetSurface((int)(Math.Round(p1.x)), (int)(Math.Round(-1 * p1.y)));

            double c = Math.Cos((source.carOrientation / 180.0) * Math.PI);
            double s = Math.Sin((source.carOrientation / 180.0) * Math.PI);
            Quad rotation = new Quad(c, -s, s, c);
            a = rotation * a;

            a.y += 0.1 * speed + 0.002 * speed * speed;
            
            Pair p = car.InputFromAccel(a, speed, f); //required input
            return p;
        }

        /// <summary>
        /// Simulates the car movement
        /// </summary>
        /// <param name="millisec"></param>
        public void Simulate(double millisec)
        {
            if (carPosition.x < 1.0) carPosition.x = 1.0;
            if (carPosition.y > -1.0) carPosition.y = -1.0;
            if (carPosition.x > Map.MapWidth - 1.0) carPosition.x = Map.MapWidth - 1.0;
            if (carPosition.y < -Map.MapHeight + 1.0) carPosition.y = -Map.MapHeight + 1.0;
            double t = millisec / 1000.0;
            if (t < EPSILON) return;
            if (throttle > 100.0) throttle = 100.0;
            else if (throttle < -100.0) throttle = -100.0;
            if (wheel > 100.0) wheel = 100.0;
            else if (wheel < -100.0) wheel = -100.0;
            Pair v = carVelocity;
            Field f = getMeanField();
            time += t;
            timePenalty += f.penalty * t;
            Pair force = car.Force(throttle, wheel, CarOrientationRadian, CarSpeed, f);
            Pair a = Accel(force);
            Pair ta = t * a;
            Pair newCarPosition = carPosition + t * (v + 0.5 * ta);
            v = v + ta;
            if ((newCarPosition - carPosition) * carOrientation < EPSILON ||
                v * carOrientation < EPSILON) // you can not back up
            {
                carVelocity = new Pair(0.0, 0.0);
            }
            else
            {
                double d = (newCarPosition - carPosition).Abs();
                distance += d;
                distancePenalty += f.penalty * d;
                carPosition = newCarPosition;
                carVelocity = v;
            }
            if (Math.Abs(carVelocity.x) < EPSILON && Math.Abs(carVelocity.y) < EPSILON)
            {
                carVelocity = new Pair(0.0, 0.0);
            }
            else
            {
                carOrientation = (1/(carVelocity.Abs()))*carVelocity;
            }
            if (carPosition.x < 1.0) carPosition.x = 1.0;
            if (carPosition.y > -1.0) carPosition.y = -1.0;
            if (carPosition.x > Map.MapWidth - 1.0) carPosition.x = Map.MapWidth - 1.0;
            if (carPosition.y < -Map.MapHeight + 1.0) carPosition.y = -Map.MapHeight + 1.0;
            updateStateVars();
            //sw.Write("Thr: "+throttle.ToString()+" Whe: "+wheel.ToString()+" Spe: "+CarSpeed+"\n");
        }

        /// <summary>
        /// Resets the state
        /// </summary>
        public void Reset()
        {
            stateVars = new List<StateVars>(STORING_DEPTH);
            carPosition = new Pair(Map.StartPos.x, -Map.StartPos.y);
            carVelocity = new Pair(0.0, 0.0);
            carOrientation = new Pair(-1.0, 0.0);
            throttle = 0.0;
            wheel = 0.0;
            car = new Car();
            time = distance = timePenalty = distancePenalty = 0.0;
            stateVars.Add(CurrentStateVars);
        }

        private Field getMeanField()
        {
            //return Map.Surfaces[Map.Fields[(int)(Math.Round(carPosition.x))-1,(int)(Math.Round(-1*carPosition.y))]-1];
            return Map.GetSurface((int)(Math.Round(carPosition.x)), (int)(Math.Round(-1 * carPosition.y)));
            //Field f = new Field();
            //f.grip = 0.9;
            //f.friction = 0.05;
            ////f.maxSpeed = 100.0;
            //f.penalty = 0.0;
            //return f;
        }

        private Pair Accel(Pair force)
        {
            force.y = force.y - 0.002 * CarSpeed * CarSpeed; //parameters.drag2
            force.y = force.y - 0.1 * CarSpeed; //parameters.drag1

            double c = Math.Cos(-CarOrientationRadian);
            double s = Math.Sin(-CarOrientationRadian);
            Quad rotation = new Quad(c, -s, s, c);
            force = rotation * force;

            return force;
        }

        private double MyAtan(double y, double x)
        {
            if (Math.Abs(x) < EPSILON)
            {
                if (Math.Abs(y) < EPSILON) throw new Exception();
                if (Math.Sign(y) > 0)
                {
                    return 0.0;
                }
                else
                {
                    return Math.PI;
                }
            }
            else if (Math.Abs(y) < EPSILON)
            {
                if (Math.Sign(x) > 0)
                {
                    return Math.PI / 2;
                }
                else
                {
                    return 3 * Math.PI / 2;
                }
            }
            else if (Math.Sign(x) > 0 && Math.Sign(y) > 0)
            {
                return Math.Atan(x / y);
            }
            else if (Math.Sign(x) > 0 && Math.Sign(y) < 0)
            {
                return Math.Atan(x / y) + (Math.PI);
            }
            else if (Math.Sign(x) < 0 && Math.Sign(y) > 0)
            {
                return Math.Atan(x / y) + 2 * (Math.PI);
            }
            else //if (Math.Sign(x) < 0 && Math.Sign(y) < 0)
            {
                return Math.Atan(x / y) + (Math.PI);
            }
        }
    }

    /*class Program
    {
        static void Main(string[] args)
        {
            StreamWriter sw = new StreamWriter("log.txt"); //File.CreateText("log.txt");
            State state = new State();
            state.Init();
            state.Throttle = 10.0;
            state.Wheel = 100.0;

            for (int i = 0; i < 100; ++i)
            {
                System.Console.WriteLine(i.ToString() + ". Position: " +
                    Math.Round(state.CarPosition.x).ToString() + ";" +
                    Math.Round(state.CarPosition.y).ToString() + "  Velocity: "+ 
                    Math.Round(state.CarVelocity.x).ToString() + ";" +
                    Math.Round(state.CarVelocity.y).ToString() + " Orientation: " +
                    Math.Round(state.CarOrientationDeg).ToString());
                sw.Write(Math.Round(state.CarPosition.x).ToString() + ";" +
                        Math.Round(state.CarPosition.y).ToString() + ";\n");
                //System.Console.ReadKey();
                state.Simulate(1.0);
            }
            sw.Close();
            //Pair p1 = new Pair(1.0, 2.0);
            //Pair p2 = new Pair(0.1, 0.2);
            //Pair p = p1 + p2;
            //System.Console.WriteLine("(1.0;2.0)+(0.1;0.2) = " + p.x.ToString() + ";" + p.y.ToString());
            //System.Console.ReadKey();
        }
    }*/
}
